//========================================================================
//  This software is free: you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License Version 3,
//  as published by the Free Software Foundation.
//
//  This software is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public License
//  Version 3 in the file COPYING that came with this distribution.
//  If not, see <http://www.gnu.org/licenses/>.
//========================================================================
/*!
\file    kinect_wrapper.cpp
\brief   C++ Implementation: KinectWrapper
\author  Joydeep Biswas, (C) 2011
*/
//========================================================================

#include "kinect_wrapper.h"

void KinectWrapper::errorInfo(HRESULT error)
{
  if(error==E_POINTER)
    printf("E_POINTER\n");
  else if(error==E_INVALIDARG)
    printf("E_INVALIDARG\n");
  else if(error==E_NUI_FEATURE_NOT_INITIALIZED)
    printf("E_NUI_FEATURE_NOT_INITIALIZED\n");
  else if(error==E_FAIL)
    printf("E_FAIL\n");
  else if(error==E_NUI_IMAGE_STREAM_IN_USE)
    printf("E_NUI_IMAGE_STREAM_IN_USE\n");
  else if(error==E_OUTOFMEMORY)
    printf("E_OUTOFMEMORY\n");
}

KinectWrapper::KinectWrapper()
{
  getDepthImg = false;
  getLabeledDepthImg = false;
  getSkeletonData = false;
  getRGBImg = false;

  depthCallback = NULL;
  labeledDepthCallback = NULL;
  skeletonCallback = NULL;
  rgbCallback = NULL;

  m_hNextDepthFrameEvent = CreateEvent( NULL, TRUE, FALSE, NULL );
  m_hNextLabeledDepthFrameEvent = CreateEvent( NULL, TRUE, FALSE, NULL );
  m_hNextVideoFrameEvent = CreateEvent( NULL, TRUE, FALSE, NULL );
}

KinectWrapper::~KinectWrapper()
{
  close();
}

int KinectWrapper::connect( void (*_depthCallback)(BYTE*), 
                            void (*_labeledDepthCallback)(BYTE*),
                            void (*_skeletonCallback)(NUI_SKELETON_FRAME*),
                            void (*_rgbCallback)(BYTE*))
{
  DWORD flags = 0;
  if(_depthCallback){
    depthCallback = _depthCallback;
    getDepthImg = true;
    flags |= NUI_INITIALIZE_FLAG_USES_DEPTH;
  }
  if(_labeledDepthCallback){
    labeledDepthCallback = _labeledDepthCallback;
    getLabeledDepthImg = true;
    flags |= NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX;
  }
  if(_skeletonCallback){
    skeletonCallback = _skeletonCallback;
    getSkeletonData = true;
    flags |= NUI_INITIALIZE_FLAG_USES_SKELETON;
  }
  if(_rgbCallback){
    rgbCallback = _rgbCallback;
    getRGBImg = true;
    flags |= NUI_INITIALIZE_FLAG_USES_COLOR;
  }
  HRESULT result = NuiInitialize( flags);
  if( result!=S_OK ){
    if(debug){
      printf("Error initializing Kinect: ");
      errorInfo(result);
    }
    close();
    return 1;
  }
  
  if(getDepthImg){
    result = NuiImageStreamOpen(
        NUI_IMAGE_TYPE_DEPTH,
        NUI_IMAGE_RESOLUTION_640x480,
        0,
        1,
        m_hNextDepthFrameEvent,
        &hDepthStream );
    if(result != S_OK){
      if(debug){
        printf("Error opening depth stream: ");
        errorInfo(result);
      }
      close();
      return 2;
    }
  }
  if(getLabeledDepthImg){
    result = NuiImageStreamOpen(
        NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX,
        NUI_IMAGE_RESOLUTION_320x240,
        0,
        1,
        m_hNextLabeledDepthFrameEvent,
        &hLabeledDepthStream );
    if(result != S_OK){
      if(debug){
        printf("Error opening labeled depth stream: ");
        errorInfo(result);
      }
      close();
      return 3;
    }
  }
  if(rgbCallback){
    result = NuiImageStreamOpen(
        NUI_IMAGE_TYPE_COLOR,
        NUI_IMAGE_RESOLUTION_640x480,
        0,
        2,
        m_hNextVideoFrameEvent,
        &hRGBStream );
    if(result != S_OK){
      if(debug){
        printf("Error opening rgb image stream: ");
        errorInfo(result);
      }
      close();
      return 4;
    }
  }
  if(getSkeletonData){
    if(NuiSkeletonTrackingEnable(NULL, 0) != S_OK){
      if(debug){
        printf("Error starting skeleton tracker: ");
        errorInfo(result);
      }
      close();
      return 5;
    }
  }

  return 0;
}

void KinectWrapper::close()
{
  getDepthImg = false;
  getLabeledDepthImg = false;
  getSkeletonData = false;
  getRGBImg = false;

  depthCallback = NULL;
  labeledDepthCallback = NULL;
  skeletonCallback = NULL;
  rgbCallback = NULL;
  if(debug) printf("Shutting down Kinect device.\n");
  NuiShutdown();
}

void KinectWrapper::poll()
{
  const NUI_IMAGE_FRAME * imgFrame = NULL;

  if(getDepthImg){
    if(NuiImageStreamGetNextFrame(hDepthStream,0,&imgFrame ) ==S_OK){
      NuiImageBuffer * imgBuffer = imgFrame->pFrameTexture;
      KINECT_LOCKED_RECT LockedRect;
      imgBuffer->LockRect( 0, &LockedRect, NULL, 0 );
      if( LockedRect.Pitch != 0 ){
        BYTE * buffer = (BYTE*) LockedRect.pBits;
        depthCallback((BYTE*) buffer );
      }
      NuiImageStreamReleaseFrame( hDepthStream, imgFrame );
    }
  }

  if(getLabeledDepthImg){
    if(NuiImageStreamGetNextFrame(hLabeledDepthStream,0,&imgFrame ) ==S_OK){
      NuiImageBuffer * imgBuffer = imgFrame->pFrameTexture;
      KINECT_LOCKED_RECT LockedRect;
      imgBuffer->LockRect( 0, &LockedRect, NULL, 0 );
      if( LockedRect.Pitch != 0 ){
        BYTE * buffer = (BYTE*) LockedRect.pBits;
        labeledDepthCallback((BYTE*) buffer );
      }
      NuiImageStreamReleaseFrame( hLabeledDepthStream, imgFrame );
    }
  }

  if(getRGBImg){
    if(NuiImageStreamGetNextFrame(hRGBStream,0,&imgFrame ) ==S_OK){
      NuiImageBuffer * imgBuffer = imgFrame->pFrameTexture;
      KINECT_LOCKED_RECT LockedRect;
      imgBuffer->LockRect( 0, &LockedRect, NULL, 0 );
      if( LockedRect.Pitch != 0 ){
        BYTE * buffer = (BYTE*) LockedRect.pBits;
        rgbCallback((BYTE*) buffer );
      }
      NuiImageStreamReleaseFrame( hRGBStream, imgFrame );
    }
  }

  if(getSkeletonData){
    if(NuiSkeletonGetNextFrame( 0, &SkeletonFrame ) == S_OK){
      long angle;
      NuiCameraElevationGetAngle(&angle);
      SkeletonFrame.vFloorClipPlane.x = double(angle);
      /*
      printf("Ground plane: %f, %f, %f, %f\n",SkeletonFrame.vFloorClipPlane.x, 
        SkeletonFrame.vFloorClipPlane.y, 
        SkeletonFrame.vFloorClipPlane.z, 
        SkeletonFrame.vFloorClipPlane.w);
      printf("Gravity: %f, %f, %f, %f\n",SkeletonFrame.vNormalToGravity.x, 
        SkeletonFrame.vNormalToGravity.y, 
        SkeletonFrame.vNormalToGravity.z, 
        SkeletonFrame.vNormalToGravity.w);
      */
      bool skeletonFound = false;
      for( int i = 0 ; !skeletonFound && i < NUI_SKELETON_COUNT ; i++ ){
        if( SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED ){
          skeletonFound = true;
        }        
      }
      if(true || skeletonFound)
        skeletonCallback(&SkeletonFrame);
    }
  }

}